
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>


#include "commKTask.h"

#include "FreeRTOS.h"
#include "task.h"

#include "nxbot.h"

extern xTaskHandle commHandle;
extern xTaskHandle soundHandle;

static void commTask(void *pvParameters);

void executeCommTask(globals_t *pvParameters)
{
	
	xTaskCreate(commTask,(const signed portCHAR * const)"commTask1",commTask_stackSize,pvParameters,tskIDLE_PRIORITY + 3,&commHandle);

}
	
static void commTask(void *pvParameters)
{
	unsigned int freq;
	portTickType actualTick;
	
	globals_t *globals;
	globals=(globals_t *)pvParameters;
	int temp=-1;
	stream commStream;	
	stream* streamPtr;
	streamPtr = &commStream;

	
	
	for(;;){
		
		//aquire the frequency for the task		
		freq=globals->times.intervalComm;
		
		while(1){
			
			if(!comm_receive(&commStream,COMM_PORT)){
				
					actualTick = xTaskGetTickCount();
					vTaskDelayUntil(&actualTick,(portTickType)freq);
			}
			else	
				break;
			
		}
		
		switch(commStream.command){
	
			//Command: B
			case GET_SW_VERSION:
			
				sprintf(streamPtr->data,",%d.%d",(int)SW_VERSION_H,(int)SW_VERSION_L);
				
				commStream.command+=32;				
				
				comm_transmit(&commStream,COMM_PORT);

			break;
			
			//Command: E
			case GET_SPEED:
				
				sprintf(streamPtr->data,",%d,%d",globals->motors.velocities.velLeftAct,globals->motors.velocities.velRightAct);

				commStream.command+=32;
								
				comm_transmit(&commStream,COMM_PORT);

			break;
			
			//Command: D
			case SET_SPEED:

				temp=sscanf(streamPtr->data,",%d,%d",&globals->motors.velocities.velLeftRef,&globals->motors.velocities.velRightRef);
				
				if(temp<2){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;

					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				
				commStream.data[0]='\0'; //No data should be returned!
				commStream.command+=32;
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: H
			case GET_POSITION:
			
				sprintf(streamPtr->data,",%f,%f,%f",(double)globals->robotPose.x,(double)globals->robotPose.y,(double)globals->robotPose.orientation);
				
				commStream.command+=32;				

				comm_transmit(&commStream,COMM_PORT);

			break;
			
			//Command: S
			case GET_ENCODERS:
			
				sprintf(streamPtr->data,",%ld,%ld",globals->motors.encoders.posLeft,globals->motors.encoders.posRight);
				
				commStream.command+=32;				

				comm_transmit(&commStream,COMM_PORT);

			break;
			
			//Command: O
			case SET_ENCODERS:
			
				temp=sscanf(streamPtr->data,",%ld,%ld",&globals->motors.encoders.posLeft,&globals->motors.encoders.posRight);
				
				//reset values
				globals->motors.encoders.lastPosLeft = globals->motors.encoders.posLeft;
				globals->motors.encoders.lastPosRight = globals->motors.encoders.posRight;
				
				if(temp<2){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				
				//reset odometers
				encoders_write(0,0);
				
				commStream.data[0]='\0'; //No data should be returned!
				commStream.command+=32;	
				
				comm_transmit(&commStream,COMM_PORT);

			break;
			
			//Command: G
			case SET_POSITION:
			
				//temp=sscanf(streamPtr->data,",%ld,%ld",&globals->robotPose.x,&globals->robotPose.y);
				temp=sscanf(streamPtr->data,",%f,%f,%f",(float*)&globals->robotPose.x,(float*)&globals->robotPose.y,(float*)&globals->robotPose.orientation);
				
				if(temp<3){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				//Yaw is a copy of orientation. It has to be inverted, thats why the minus. See motorsTask where orientation = -yaw.
				globals->robotPose.yaw = - (globals->robotPose.orientation);
				
				//reset odometers
				encoders_write(0,0);
				
				//reset values of encoders
				globals->motors.encoders.posLeft = 0;
				globals->motors.encoders.posRight = 0;
				globals->motors.encoders.lastPosLeft = 0;
				globals->motors.encoders.lastPosRight = 0;
				
				
				commStream.data[0]='\0'; //No data should be returned!
				commStream.command+=32;	
				
				comm_transmit(&commStream,COMM_PORT);

			break;
			
			//Command: N
			case GET_IRSENSOR:
			
				sprintf(streamPtr->data,",%d",globals->sensors.sensorsIR);
				
				commStream.command+=32;				
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: W
			case GET_SONARS:
			
				//TODO: Esta funcion debe permitir ser variable ne parametros, de acuerdo a la cantidad de sonares conectados
				#if NXBOT_SONAR_COUNT == 3
				sprintf(streamPtr->data,",%d,%d,%d",globals->sensors.sonars[0],globals->sensors.sonars[1],globals->sensors.sonars[2]);
				#elif NXBOT_SONAR_COUNT == 5
				sprintf(streamPtr->data,",%d,%d,%d,%d,%d",globals->sensors.sonars[0],globals->sensors.sonars[1],globals->sensors.sonars[2],globals->sensors.sonars[3],globals->sensors.sonars[4]);
				#elif NXBOT_SONAR_COUNT == 6
				sprintf(streamPtr->data,",%d,%d,%d,%d,%d,%d",globals->sensors.sonars[0],globals->sensors.sonars[1],globals->sensors.sonars[2],globals->sensors.sonars[3],globals->sensors.sonars[4],globals->sensors.sonars[5]);
				#elif NXBOT_SONAR_COUNT == 8
				sprintf(streamPtr->data,",%d,%d,%d,%d,%d,%d,%d,%d",globals->sensors.sonars[0],globals->sensors.sonars[1],globals->sensors.sonars[2],globals->sensors.sonars[3],globals->sensors.sonars[4],globals->sensors.sonars[5],globals->sensors.sonars[6],globals->sensors.sonars[7]);
				#endif
				
				commStream.command+=32;				
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: M
			case GET_BUMPSENSOR:
			
				sprintf(streamPtr->data,",%d",globals->sensors.bumpers);
				
				commStream.command+=32;				
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: Q
			case GET_BATT_STATE:
			
				sprintf(streamPtr->data,",%d,%d",globals->sensors.batt.battState,globals->sensors.batt.battVoltage);
				
				commStream.command+=32;				
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			
			//Command: C
			case GET_ADC_READ:
			
				sprintf(streamPtr->data,",%d,%d,%d,%d,%d,%d,%d,%d",globals->sensors.adc[0],globals->sensors.adc[1],globals->sensors.adc[2],globals->sensors.adc[3],globals->sensors.adc[4],globals->sensors.adc[5],globals->sensors.adc[6],globals->sensors.adc[7]);
				
				commStream.command+=32;				
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			
			//Command: P
			case SET_VEL_CONTROL:
				
				temp = sscanf(streamPtr->data,",%f,%f,%f",(float*)&globals->parametersPI.Kp,(float*)&globals->parametersPI.Ti,(float*)&globals->parametersPI.Td);
				
				if(temp<3){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			
			//Command: I
			case GET_VEL_CONTROL:
				
				sprintf(streamPtr->data,",%.2f,%.2f,%.2f",globals->parametersPI.Kp,globals->parametersPI.Ti,globals->parametersPI.Td);
				

				commStream.command+=32;	

				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: F
			case GET_SONARCOUNT:
				
				sprintf(streamPtr->data,",%d",globals->sensors.sonarCount);

				commStream.command+=32;	

				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: A
			case SET_SONARCOUNT:
				
				temp = sscanf(streamPtr->data,",%d",(int*)&globals->sensors.sonarCount);
				
				if(temp<1){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: R
			case SET_SOUND_JINGLE:
				
				sscanf(streamPtr->data,",%d",&globals->soundJingle);			
				

				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				comm_transmit(&commStream,COMM_PORT);
				vTaskResume( soundHandle );
			break;

			//Command: J
			case READ_DIO:
				
				
				sprintf(streamPtr->data,",%d",globals->sensors.dioIn);

				commStream.command+=32;	

				comm_transmit(&commStream,COMM_PORT);
			break;

			//Command: K
			case WRITE_DIO:
				
				sscanf(streamPtr->data,",%d",(int*)&globals->sensors.dioOut);	
				
				if(temp<1){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				comm_transmit(&commStream,COMM_PORT);
			break;
			
			//Command: L
			case SET_GRIPPER:
				
				temp = sscanf(streamPtr->data,",%d",(int*)&globals->gripperState);
				
				if(temp<1){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					comm_transmit(&commStream,COMM_PORT);
					
					break;
				}
				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				comm_transmit(&commStream,COMM_PORT);
			break;

						
			default:

				commStream.command=COMMUNICATION_ERROR;
				
				comm_transmit(&commStream,COMM_PORT);
			break;
		}
		
	}
}

